#include "../common/irobot_abstract2.h"
#include "../common/oi.h"

#define bound_velocity(x) (x <= 500 ? x : (x >= 0 ? x : -500))


//create office theme song as song number 0
void create_office_theme_song(void)
{
	ir_send_byte(CmdSong);
	ir_send_byte(0);
	ir_send_byte(14);
	ir_send_byte(67);
	ir_send_byte(32);
	ir_send_byte(79);
	ir_send_byte(64);
	ir_send_byte(78);
	ir_send_byte(8);
	ir_send_byte(79);
	ir_send_byte(8);
	ir_send_byte(78);
	ir_send_byte(8);
	ir_send_byte(74);
	ir_send_byte(8);
	ir_send_byte(76);
	ir_send_byte(64);
	ir_send_byte(72);
	ir_send_byte(16);
	ir_send_byte(72);
	ir_send_byte(16);
	ir_send_byte(72);
	ir_send_byte(8);
	ir_send_byte(71);
	ir_send_byte(8);
	ir_send_byte(69);
	ir_send_byte(8);
	ir_send_byte(71);
	ir_send_byte(8);
	ir_send_byte(67);
	ir_send_byte(64);
}

//play office theme song
void play_office_theme_song(void)
{
	ir_send_byte(CmdPlay);
	ir_send_byte(0);
}


enum state_t {find_wall, backup, orient_wall, follow_wall, halt, dock, tractor_beam,jiggle};

enum state_t f_find_wall(void){

	if(left_bumper() || right_bumper()){
		stop();
		return backup;
	}

	if(infrared() == 248 ||
		infrared() == 244 ||
		infrared() == 252 ||
		infrared() == 250 ||
		infrared() == 246 ||
		infrared() == 254){
		stop();
		return dock;
	} else if(infrared() == 242){
		turn_in_place(100,0);
	} else {
		drive(250,250);
	}

	return find_wall;
}

enum state_t f_backup(void){
	drive(-50,-50);

	if(!left_bumper() && !right_bumper()){
		ir_sleep(30);
		return orient_wall;
	}

	return backup;
}

enum state_t f_orient_wall(void){
	drive(50,-50);

	static int threshold = 130.0;

	if(wall_signal() > (int)threshold){
		stop();
		return follow_wall;
	}
	//threshold -= 0.1;

	return orient_wall;
}

enum state_t f_follow_wall(void){
	static int right_wheel,left_wheel;

	if(infrared() == 248 ||
		infrared() == 244 ||
		infrared() == 252 ||
		infrared() == 250 ||
		infrared() == 246 ||
		infrared() == 254){
		stop();
		return dock;
	} else if(infrared() == 242){
		stop();
		return tractor_beam;
	}

	if(left_bumper()){
		stop();
		return backup;
	}

	int wall = wall_signal();
	if(wall <= 4095) {
		if(right_bumper())
			left_wheel = -100;
		else
			left_wheel = 250 - (wall-90)*(wall < 90 ? 3 : 1);
		right_wheel = 250 + (wall-90)*(wall < 90 ? 2 : 1);
		drive(bound_velocity(right_wheel),bound_velocity(left_wheel));
	}

	return follow_wall;
}

enum state_t f_dock(void){
	if(infrared() == 242){
		leds(false,false,0,255);

		drive(50,50);
	}
	else if(infrared() == 248){ //red
		leds(false,true,0,0);

		drive(100,0);
	}
	else if(infrared() == 244){ //green
		leds(true,false,0,0);

		drive(0,100);
	}
	else if(infrared() == 252){ // red&green
		leds(true,true,0,0);

		drive(100,100);
	}
	else if(infrared() == 250){ // red&force
		leds(false,true,0,255);

		drive(100,0);
	}
	else if(infrared() == 246){ // green&force
		leds(true,false,0,255);

		drive(0,100);
	}
	else if(infrared() == 254){ // all
		leds(true,true,0,255);

		drive(100,100);
	}
	else{
		leds(false,false,0,0);

		return find_wall;
	}

	if(left_bumper()||right_bumper()){
		//stop();
		//return halt;
		stop();
		return jiggle;
	}


	return dock;
}

enum state_t f_tractor_beam(void){
	if(infrared() == 244 || infrared() == 246){
		stop();
		return dock;
	}
	static bool first = true;

	if(first){
		turn_in_place(100,1);
		ir_sleep(time_to_rotate_degrees(100,90));
		first = false;
	}else{
		turn_about(100, -400);
	}
			

	return tractor_beam;
}

enum state_t f_jiggle(void){

	//ir_sleep(500);
	
	int i;
	for(i = 0; i < 50; ++i){
		collect_sensor_data();
	}

	if(charging_sources() > 0){
		stop();
		leds(true,true,255,255);
		
		//return jiggle;
		return halt;
	}
	else
	{
		leds(false,false,0,0);
		
		drive(-150, -150);
		ir_sleep(2000);
		stop();
		return dock;
	}

	return jiggle;
}




int main(int argc, char **argv) {
	ir_initialize(0);
	ir_sleep(100);
	
	ir_send_byte(CmdStart);
	ir_send_byte(CmdFull);

/// light the leds--idiot test
	ir_sleep(100);
	leds(false,false,0,255);
	/*
	ir_send_byte(CmdLeds);
	ir_send_byte(0);
	ir_send_byte(0);
	ir_send_byte(255);*/

	create_office_theme_song();

	enum state_t state = find_wall;
	while(state != halt) {
		ir_sleep(100);
		collect_sensor_data();

		switch (state){
			case find_wall: state = f_find_wall(); break;
			case backup: state = f_backup(); break;
			case orient_wall: state = f_orient_wall(); break;
			case follow_wall: state= f_follow_wall(); break;
			case dock: state= f_dock(); break;
			case tractor_beam: state= f_tractor_beam(); break;
			case jiggle: state= f_jiggle(); break;
		}
	}

	play_office_theme_song();

	ir_finish();
}
